#ifndef ANALYTIC_REPROJECTION_ERROR_H
#define ANALYTIC_REPROJECTION_ERROR_H

#include <ceres/ceres.h>
#include <ceres/cost_function.h>
#include <Eigen/Dense>

class AnalyticReprojectionError : public ceres::SizedCostFunction<2, 3, 4> {
public:
    AnalyticReprojectionError(const Eigen::Vector3d& landmark, 
                             const Eigen::Vector2d& bearing,
                             double fx, double fy, double cx, double cy)
        : landmark_(landmark), bearing_(bearing), fx_(fx), fy_(fy), cx_(cx), cy_(cy) {
    }

    AnalyticReprojectionError(const Eigen::Vector3d& point_3d, double observed_x, double observed_y, 
                             double fx, double fy, double cx, double cy)
        :  point_3d_(point_3d), observed_x_(observed_x), observed_y_(observed_y),
          fx_(fx), fy_(fy), cx_(cx), cy_(cy) {}

    virtual bool Evaluate(double const* const* parameters,
                         double* residuals,
                         double** jacobians) const override {
        // method 2
        // 提取参数
        const double* translation = parameters[0];
        const double* quaternion = parameters[1]; // [x, y, z, w] 格式

        // 构造四元数 (注意Eigen的顺序是[x, y, z, w])
        Eigen::Quaterniond q(quaternion[3], quaternion[0], quaternion[1], quaternion[2]);
        q.normalize();
        
        Eigen::Matrix3d R = q.toRotationMatrix();

        // 将3D点从世界坐标系变换到相机坐标系
        Eigen::Vector3d p_c = R * landmark_ + Eigen::Map<const Eigen::Vector3d>(translation);
        
        double x_c = p_c[0];
        double y_c = p_c[1];
        double z_c = p_c[2];

        // 避免除零
        if (z_c < 1e-8) {
            z_c = 1e-8;
        }

        double inv_z = 1.0 / z_c;
        double inv_z2 = inv_z * inv_z;

        // 投影到归一化平面
        double x_n = x_c * inv_z;
        double y_n = y_c * inv_z;
        
        // 应用内参，得到预测的像素坐标
        double u_pred = fx_ * x_n + cx_;
        double v_pred = fy_ * y_n + cy_;
        
        // 计算残差
        residuals[0] = bearing_[0] - u_pred;
        residuals[1] = bearing_[1] - v_pred;

        // 如果不需要计算雅可比，直接返回
        if (jacobians == nullptr) {
            return true;
        }

        // 计算关于平移的雅可比
        if (jacobians[0] != nullptr) {
            ComputeTranslationJacobian(x_c, y_c, z_c, inv_z, inv_z2, jacobians[0]);
        }

        // 计算关于四元数的雅可比
        if (jacobians[1] != nullptr) {
            ComputeQuaternionJacobian(x_c, y_c, z_c, inv_z, inv_z2, R, quaternion, jacobians[1]);
            // ComputeQuaternionJacobianMethod2(x_c, y_c, z_c, inv_z, inv_z2, R, quaternion, jacobians[1]);
        }

        return true;
    }

private:
    void ComputeTranslationJacobian(double x_c, double y_c, double z_c, 
                                   double inv_z, double inv_z2, double* jacobian) const {
        // ∂r/∂t = -J_proj * ∂p_c/∂t = -J_proj * I
        
        // J_proj = [ fx/z_c,   0,    -fx*x_c/z_c^2 ]
        //          [   0,    fy/z_c, -fy*y_c/z_c^2 ]
        
        // 所以 ∂r/∂t = -J_proj
        
        jacobian[0] = -fx_ * inv_z;                    // ∂r_u/∂t_x
        jacobian[1] = 0.0;                             // ∂r_u/∂t_y  
        jacobian[2] = fx_ * x_c * inv_z2;              // ∂r_u/∂t_z

        jacobian[3] = 0.0;                             // ∂r_v/∂t_x
        jacobian[4] = -fy_ * inv_z;                    // ∂r_v/∂t_y
        jacobian[5] = fy_ * y_c * inv_z2;              // ∂r_v/∂t_z
    }

    void ComputeQuaternionJacobian(double x_c, double y_c, double z_c,
                                  double inv_z, double inv_z2,
                                  const Eigen::Matrix3d& R,
                                  const double* quaternion,
                                  double* jacobian) const {
        
        const double& qw = quaternion[3]; // w
        const double& qx = quaternion[0]; // x
        const double& qy = quaternion[1]; // y  
        const double& qz = quaternion[2]; // z

        const double& X = landmark_[0];
        const double& Y = landmark_[1];
        const double& Z = landmark_[2];

        double z_c2 = z_c * z_c;
        Eigen::Matrix<double, 2, 3> J_proj;
        J_proj << fx_/z_c, 0.0, -fx_*x_c/z_c2,
                  0.0, fy_/z_c, -fy_*y_c/z_c2;

        // 计算 ∂p_c/∂q (3x4) - 根据之前的推导公式
        Eigen::Matrix<double, 3, 4> J_pc_q;
        
        // 方法1： 对 qx 的导数
        J_pc_q(0, 0) = 2.0 * ( qy*Y + qz*Z);
        J_pc_q(1, 0) = 2.0 * ( qy*X - 2*qx*Y - qw*Z);
        J_pc_q(2, 0) = 2.0 * ( qz*X + qw*Y - 2*qx*Z);

        // 对 qy 的导数
        J_pc_q(0, 1) = 2.0 * (-2*qy*X + qx*Y + qw*Z);
        J_pc_q(1, 1) = 2.0 * ( qx*X + qz*Z);
        J_pc_q(2, 1) = 2.0 * (-qw*X + qz*Y - 2*qy*Z);

        // 对 qz 的导数
        J_pc_q(0, 2) = 2.0 * (-2*qz*X - qw*Y + qx*Z);
        J_pc_q(1, 2) = 2.0 * ( qw*X - 2*qz*Y + qy*Z);
        J_pc_q(2, 2) = 2.0 * ( qx*X + qy*Y);

        // 对 qw 的导数
        J_pc_q(0, 3) = 2.0 * (-qz*Y + qy*Z);
        J_pc_q(1, 3) = 2.0 * ( qz*X - qx*Z);
        J_pc_q(2, 3) = 2.0 * (-qy*X + qx*Y);

        // 组合雅可比: ∂r/∂q = -J_proj * J_pc_q
        Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> J_r_q(jacobian);
        J_r_q = -J_proj * J_pc_q;   
    }

    void ComputeQuaternionJacobianMethod2(double x_c, double y_c, double z_c,
                                  double inv_z, double inv_z2,
                                  const Eigen::Matrix3d& R,
                                  const double* quaternion,
                                  double* jacobian) const {
        
        const double& qw = quaternion[3]; // w
        const double& qx = quaternion[0]; // x
        const double& qy = quaternion[1]; // y  
        const double& qz = quaternion[2]; // z

        const double& X = landmark_[0];
        const double& Y = landmark_[1];
        const double& Z = landmark_[2];

        Eigen::Matrix<double, 2, 3> J_proj;
        J_proj << fx_ * inv_z, 0.0, -fx_ * x_c * inv_z2,
                0.0, fy_ * inv_z, -fy_ * y_c * inv_z2;

        // 方法2：使用四元数旋转的另一种导数表示
        Eigen::Matrix<double, 3, 4> J_pc_q;
    
        // 构建四元数矩阵表示
        // p_c = q ⊗ p_w ⊗ q*
        // 关于四元数的导数可以表示为：
    
        J_pc_q(0, 0) = 0.0;         J_pc_q(0, 1) = 2.0*Z;      J_pc_q(0, 2) = -2.0*Y;     J_pc_q(0, 3) = 2.0*X;
        J_pc_q(1, 0) = -2.0*Z;      J_pc_q(1, 1) = 0.0;        J_pc_q(1, 2) = 2.0*X;      J_pc_q(1, 3) = 2.0*Y;
        J_pc_q(2, 0) = 2.0*Y;       J_pc_q(2, 1) = -2.0*X;     J_pc_q(2, 2) = 0.0;        J_pc_q(2, 3) = 2.0*Z;

        // 应用四元数
        Eigen::Matrix4d Q_mat;
        Q_mat <<  qw, -qz,  qy,  qx,
                  qz,  qw, -qx,  qy,
                  -qy,  qx,  qw,  qz,
                 -qx, -qy, -qz,  qw;
    
        J_pc_q = J_pc_q * Q_mat;

        // 组合雅可比: ∂r/∂q = -J_proj * J_pc_q
        Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> J_r_q(jacobian);
        J_r_q = -J_proj * J_pc_q;

    }
    const Eigen::Vector3d landmark_;
    const Eigen::Vector2d bearing_;
    Eigen::Vector3d point_3d_;
    double observed_x_, observed_y_;
    const double fx_, fy_, cx_, cy_;
};

#endif // ANALYTIC_REPROJECTION_ERROR_H